// -*- c++ -*-
// Copyright 2008 Isis Innovation Limited

#ifndef __TRACKERDATA_H
#define __TRACKERDATA_H

#include "PatchFinder.h"
#include "ATANCamera.h"

// This class contains all the intermediate results associated with
// a map-point that the tracker keeps up-to-date. TrackerData
// basically handles all the tracker's point-projection jobs,
// and also contains the PatchFinder which does the image search.
// It's very code-heavy for an h-file (it's a bunch of methods really)
// but it's only included from Tracker.cc!

namespace PTAM {

struct TrackerData {
    TrackerData(MapPoint* pMapPoint)
            : Point(*pMapPoint) { };

    MapPoint& Point;
    PatchFinder Finder;

    // Projection itermediates:
    Vector<3> v3Cam;        // Coords in current cam frame
    Vector<2> v2ImPlane;    // Coords in current cam z=1 plane
    Vector<2> v2Image;      // Pixel coords in LEVEL0
    Matrix<2> m2CamDerivs;  // Camera projection derivs
    bool bInImage;
    bool bPotentiallyVisible;

    int nSearchLevel;
    bool bSearched;
    bool bFound;
    bool bDidSubPix;
    Vector<2> v2Found;      // Pixel coords of found patch (L0)
    DefaultPrecision dSqrtInvNoise;   // Only depends on search level..

    // Stuff for pose update:
    Vector<2> v2Error_CovScaled;
    Matrix<2, 6> m26Jacobian;   // Jacobian wrt camera position

    // Project point into image given certain pose and camera.
    // This can bail out at several stages if the point
    // will not be properly in the image.
    inline void Project(const SE3<>& se3CFromW, ATANCamera& Cam) {
        bInImage = bPotentiallyVisible = false;
        v3Cam = se3CFromW * Point.v3WorldPos;
        if (v3Cam[2] < 0.001)
            return;
        v2ImPlane = project(v3Cam);
        if ((v2ImPlane * v2ImPlane) > (Cam.LargestRadiusInImage() * Cam.LargestRadiusInImage()))
            return;
        v2Image = Cam.Project(v2ImPlane);
        if (Cam.Invalid())
            return;

        if (v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] ||
            v2Image[1] > irImageSize[1])
            return;
        bInImage = true;
    }

    // Get the projection derivatives (depend only on the camera.)
    // This is called Unsafe because it depends on the camera caching
    // results from the previous projection:
    // Only do this right after the same point has been projected!
    inline void GetDerivsUnsafe(ATANCamera& Cam) {
        m2CamDerivs = Cam.GetProjectionDerivs();
    }

    // Does projection and gets camera derivs all in one.
    inline void ProjectAndDerivs(SE3<>& se3, ATANCamera& Cam) {
        Project(se3, Cam);
        if (bFound) {
            GetDerivsUnsafe(Cam);
        }
    }

    // Jacobian of projection W.R.T. the camera position
    // I.e. if  p_cam = SE3Old * p_world,
    //         SE3New = SE3Motion * SE3Old
    inline void CalcJacobian() {
        DefaultPrecision dOneOverCameraZ = 1.0 / v3Cam[2];
        for (int m = 0; m < 6; m++) {
            const Vector<4> v4Motion = SE3<>::generator_field(m, unproject(v3Cam));
            Vector<2> v2CamFrameMotion;
            v2CamFrameMotion[0] =
                    (v4Motion[0] - v3Cam[0] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
            v2CamFrameMotion[1] =
                    (v4Motion[1] - v3Cam[1] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
            m26Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion;
        }
    }

    // Sometimes in tracker instead of reprojecting, just update the error linearly!
    inline void LinearUpdate(const Vector<6>& v6) {
        v2Image += m26Jacobian * v6;
    }

    // This static member is filled in by the tracker and allows in-image checks in this class above.
    static CVD::ImageRef irImageSize;
};


}

#endif




